/*
 * Copyright 2016-2020 Autoware Foundation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/*
 * ROSHelpers.h
 *
 *  Created on: Jun 30, 2016
 *      Author: Hatem Darweesh
 */

#ifndef ROSHELPERS_H_
#define ROSHELPERS_H_

#include <ros/ros.h>

#include "vector_map_msgs/PointArray.h"
#include "vector_map_msgs/LaneArray.h"
#include "vector_map_msgs/NodeArray.h"
#include "vector_map_msgs/StopLineArray.h"
#include "vector_map_msgs/DTLaneArray.h"

#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>

#include "libwaypoint_follower/libwaypoint_follower.h"
#include "autoware_msgs/LaneArray.h"

#include <visualization_msgs/MarkerArray.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/tf.h>
#include "op_planner/RoadNetwork.h"

#include <string>
#include <vector>

namespace WayPlannerNS
{
enum MSG_TYPE{COMMAND_MSG = 0, CONFIRM_MSG = 1, OPTIONS_MSG = 2, CURR_OPTION_MSG = 3, UNKNOWN_MSG = 5};

class HMI_MSG
{
  public:
    MSG_TYPE type;
    std::vector<PlannerHNS::ACTION_TYPE> options;
    PlannerHNS::ACTION_TYPE current;
    int currID;
    bool bErr;
    std::string err_msg;
    HMI_MSG()
    {
      currID = -1;
      type = OPTIONS_MSG;
      current = PlannerHNS::FORWARD_ACTION;
      bErr = false;
    }

    static HMI_MSG FromString(std::string msg)
    {
      HMI_MSG recieved_msg;
      std::vector<std::string> sections = SplitString(msg, ",");
      if (sections.size() == 6)
      {
        int type_str = atoi(sections.at(0).c_str());
        switch (type_str)
        {
        case 0:
          recieved_msg.type = COMMAND_MSG;
          break;
        case 1:
          recieved_msg.type = CONFIRM_MSG;
          break;
        case 2:
          recieved_msg.type = OPTIONS_MSG;
          break;
        case 3:
          recieved_msg.type = CURR_OPTION_MSG;
          break;
        default:
          recieved_msg.type = UNKNOWN_MSG;
          break;
        }

        std::vector<std::string> directions = SplitString(sections.at(1), ";");
        for (unsigned int i = 0; i < directions.size(); i++)
        {
          int idirect = atoi(directions.at(i).c_str());
          if (idirect == 0)
          {
            recieved_msg.options.push_back(PlannerHNS::FORWARD_ACTION);
          }
          else if (idirect == 3)
          {
            recieved_msg.options.push_back(PlannerHNS::LEFT_TURN_ACTION);
          }
          else if (idirect == 4)
          {
            recieved_msg.options.push_back(PlannerHNS::RIGHT_TURN_ACTION);
          }
        }
        recieved_msg.currID = atoi(sections.at(3).c_str());
        recieved_msg.bErr = atoi(sections.at(4).c_str());
        recieved_msg.err_msg = sections.at(5);
      }
      return recieved_msg;
    }

    static std::vector<std::string> SplitString(const std::string& str, const std::string& token)
    {
      std::vector<std::string> str_parts;
      int iFirstPart = 0;
      int iSecondPart = str.find(token, iFirstPart);

      while ((iSecondPart > 0) && (iSecondPart < str.size()))
      {
        str_parts.push_back(str.substr(iFirstPart, iSecondPart - iFirstPart));
        iFirstPart = iSecondPart+1;
        iSecondPart = str.find(token, iFirstPart);
      }

      return str_parts;
    }
};

class ROSHelpers
{
  public:
    ROSHelpers();
    virtual ~ROSHelpers();
    static void GetTransformFromTF(const std::string parent_frame, const std::string child_frame,
          tf::StampedTransform &transform);
    static void ConvertFromPlannerHToAutowarePathFormat(const std::vector<PlannerHNS::WayPoint>& path,
          autoware_msgs::LaneArray& laneArray);

    static void ConvertFromPlannerHToAutowareVisualizePathFormat(const std::vector<PlannerHNS::WayPoint>& curr_path,
        const std::vector<std::vector<PlannerHNS::WayPoint>>& paths,
          visualization_msgs::MarkerArray& markerArray);

    static void ConvertFromPlannerHToAutowareVisualizePathFormat(
          const std::vector<std::vector<PlannerHNS::WayPoint>>& globalPaths,
          visualization_msgs::MarkerArray& markerArray);

    static void ConvertFromRoadNetworkToAutowareVisualizeMapFormat(const PlannerHNS::RoadNetwork& map,
          visualization_msgs::MarkerArray& markerArray);

    static void createGlobalLaneArrayMarker(std_msgs::ColorRGBA color,
          const autoware_msgs::LaneArray &lane_waypoints_array, visualization_msgs::MarkerArray& markerArray);

    static void createGlobalLaneArrayVelocityMarker(const autoware_msgs::LaneArray &lane_waypoints_array,
          visualization_msgs::MarkerArray& markerArray);

    static void createGlobalLaneArrayOrientationMarker(const autoware_msgs::LaneArray &lane_waypoints_array,
          visualization_msgs::MarkerArray& markerArray);

    static void ConvertFromPlannerHPointsToAutowarePathFormat(const std::vector<PlannerHNS::GPSPoint>& path,
        autoware_msgs::LaneArray& laneArray);

    static void FindIncommingBranches(const std::vector<std::vector<PlannerHNS::WayPoint>>& globalPaths,
        const PlannerHNS::WayPoint& currPose, const double& min_distance,
        std::vector<PlannerHNS::WayPoint*>& branches, PlannerHNS::WayPoint* currOptions);
};

}  // namespace WayPlannerNS

#endif  // ROSHELPERS_H_
